﻿/// QuaterniondNUnit.cs
/// 
/// Repeat random values to test whether the output of the method in struct Quaterniond and struct Quaternion is similar.
/// 
/// 重复随机取值，测试Quaterniond与Quaternion中方法的输出是否近似。
/// 
/// Created by D子宇 on 2018.3.17
/// 
/// Email: darkziyu@126.com
using System;
using Mathd;
using NUnit.Framework;
using UnityEngine;
using FixedPointMath;

[TestFixture]
public class QuaternionNUnit {
    private const double deviation = 0.1;
    private const int count = 999999;


    [Test]
    [Category("Quaterniond")]
    public void Angle()
    {
        for (int i = 0; i < count; i++)
        {
            UnityEngine.Quaternion a = new UnityEngine.Quaternion();
            UnityEngine.Quaternion b = new UnityEngine.Quaternion();
            Quaterniond ad = new Quaterniond();
            Quaterniond bd = new Quaterniond();

            RandomQuaternion(ref a, ref ad);
            RandomQuaternion(ref b, ref bd);

            float value = UnityEngine.Quaternion.Angle(a, b);
            double valued = Quaterniond.Angle(ad, bd);

            NUnit.Framework.Assert.True(Approximate(value, valued));
        }
    }

    [Test]
    [Category("Quaterniond")]
    public void AngleAxis()
    {
        for (int i = 0; i < count; i++)
        {
            float ax, ay, az;
            float p;

            ax = UnityEngine.Random.Range(0, 360F);
            ay = UnityEngine.Random.Range(0, 360F);
            az = UnityEngine.Random.Range(0, 360F);

            p = UnityEngine.Random.Range(0, 360F);

            UnityEngine.Quaternion value = UnityEngine.Quaternion.AngleAxis(p, new UnityEngine.Vector3(ax, ay, az));
            Quaterniond valued = Quaterniond.AngleAxis(p, new Vector3d(ax, ay, az));

            NUnit.Framework.Assert.True(Approximate(value, valued));
        }
    }

    [Test]
    [Category("Quaterniond")]
    public void Dot()
    {
        for (int i = 0; i < count; i++)
        {
            UnityEngine.Quaternion a = new UnityEngine.Quaternion();
            UnityEngine.Quaternion b = new UnityEngine.Quaternion();
            Quaterniond ad = new Quaterniond();
            Quaterniond bd = new Quaterniond();

            RandomQuaternion(ref a, ref ad);
            RandomQuaternion(ref b, ref bd);

            float value = UnityEngine.Quaternion.Dot(a, b);
            double valued = Quaterniond.Dot(ad, bd);

            NUnit.Framework.Assert.True(Approximate(value, valued));
        }
    }

    [Test]
    [Category("Quaterniond")]
    public void Euler()
    {
        for (int i = 0; i < count; i++)
        {
            float ax, ay, az;

            ax = UnityEngine.Random.Range(0, 360F);
            ay = UnityEngine.Random.Range(0, 360F);
            az = UnityEngine.Random.Range(0, 360F);

            UnityEngine.Quaternion value = UnityEngine.Quaternion.Euler(new UnityEngine.Vector3(ax, ay, az));
            Quaterniond valued = Quaterniond.Euler(new Vector3d(ax, ay, az));

            NUnit.Framework.Assert.True(Approximate(value, valued));
        }
    }

    //[Test]
    //[Category("Quaterniond")]
    //public void FromToRotation()
    //{
    //    for (int i = 0; i < count; i++)
    //    {
    //        float ax, ay, az;
    //        float bx, by, bz;

    //        ax = UnityEngine.Random.Range(-10F, 10F);
    //        ay = UnityEngine.Random.Range(-10F, 10F);
    //        az = UnityEngine.Random.Range(-10F, 10F);

    //        bx = UnityEngine.Random.Range(-10F, 10F);
    //        by = UnityEngine.Random.Range(-10F, 10F);
    //        bz = UnityEngine.Random.Range(-10F, 10F);

    //        Quaternion value = Quaternion.FromToRotation(new Vector3(ax, ay, az), new Vector3(bx, by, bz));
    //        Quaterniond valued = Quaterniond.FromToRotation(new Vector3d(ax, ay, az), new Vector3d(bx, by, bz));

    //        Assert.True(Approximate(value, valued));
    //    }
    //}

    [Test]
    [Category("Quaterniond")]
    public void Inverse()
    {
        for (int i = 0; i < count; i++)
        {
            UnityEngine.Quaternion a = new UnityEngine.Quaternion();
            Quaterniond ad = new Quaterniond();

            RandomQuaternion(ref a, ref ad);

            UnityEngine.Quaternion value = UnityEngine.Quaternion.Inverse(a);
            Quaterniond valued = Quaterniond.Inverse(ad);

            NUnit.Framework.Assert.True(Approximate(value, valued));
        }
    }

    [Test]
    [Category("Quaterniond")]
    public void Lerp()
    {
        for (int i = 0; i < count; i++)
        {
            UnityEngine.Quaternion a = new UnityEngine.Quaternion();
            UnityEngine.Quaternion b = new UnityEngine.Quaternion();
            Quaterniond ad = new Quaterniond();
            Quaterniond bd = new Quaterniond();

            RandomQuaternion(ref a, ref ad);
            RandomQuaternion(ref b, ref bd);

            float p = UnityEngine.Random.Range(-2F, 2F);

            UnityEngine.Quaternion value = UnityEngine.Quaternion.Lerp(a, b, p);
            Quaterniond valued = Quaterniond.Lerp(ad, bd, p);

            NUnit.Framework.Assert.True(Approximate(value, valued));
        }
    }

    [Test]
    [Category("Quaterniond")]
    public void LerpUnclamped()
    {
        for (int i = 0; i < count; i++)
        {
            UnityEngine.Quaternion a = new UnityEngine.Quaternion();
            UnityEngine.Quaternion b = new UnityEngine.Quaternion();
            Quaterniond ad = new Quaterniond();
            Quaterniond bd = new Quaterniond();

            RandomQuaternion(ref a, ref ad);
            RandomQuaternion(ref b, ref bd);

            float p = UnityEngine.Random.Range(2F, 3F);

            UnityEngine.Quaternion value = UnityEngine.Quaternion.LerpUnclamped(a, b, p);
            Quaterniond valued = Quaterniond.LerpUnclamped(ad, bd, p);

            NUnit.Framework.Assert.True(Approximate(value, valued));
        }
    }

    [Test]
    [Category("Quaterniond")]
    public void LookRotation()
    {
        for (int i = 0; i < count; i++)
        {
            float ax, ay, az;

            ax = UnityEngine.Random.Range(-10F, 10F);
            ay = UnityEngine.Random.Range(-10F, 10F);
            az = UnityEngine.Random.Range(-10F, 10F);

            UnityEngine.Quaternion value = UnityEngine.Quaternion.LookRotation(new UnityEngine.Vector3(ax, ay, az));
            Quaterniond valued = Quaterniond.LookRotation(new Vector3d(ax, ay, az));

            NUnit.Framework.Assert.True(Approximate(value, valued));
        }
    }

    [Test]
    [Category("Quaterniond")]
    public void LookRotation1()
    {
        for (int i = 0; i < count; i++)
        {
            float ax, ay, az;
            float bx, by, bz;

            ax = UnityEngine.Random.Range(-10F, 10F);
            ay = UnityEngine.Random.Range(-10F, 10F);
            az = UnityEngine.Random.Range(-10F, 10F);

            bx = UnityEngine.Random.Range(-10F, 10F);
            by = UnityEngine.Random.Range(-10F, 10F);
            bz = UnityEngine.Random.Range(-10F, 10F);

            UnityEngine.Quaternion value = UnityEngine.Quaternion.LookRotation(new UnityEngine.Vector3(ax, ay, az), new UnityEngine.Vector3(bx, by, bz));
            Quaterniond valued = Quaterniond.LookRotation(new Vector3d(ax, ay, az), new Vector3d(bx, by, bz));

            NUnit.Framework.Assert.True(Approximate(value, valued));
        }
    }

    [Test]
    [Category("Quaterniond")]
    public void RotateTowards()
    {
        for (int i = 0; i < count; i++)
        {
            UnityEngine.Quaternion a = new UnityEngine.Quaternion();
            UnityEngine.Quaternion b = new UnityEngine.Quaternion();
            Quaterniond ad = new Quaterniond();
            Quaterniond bd = new Quaterniond();

            float p = UnityEngine.Random.Range(-2F, 2F);

            RandomQuaternion(ref a, ref ad);
            RandomQuaternion(ref b, ref bd);

            UnityEngine.Quaternion value = UnityEngine.Quaternion.RotateTowards(a, b, p);
            Quaterniond valued = Quaterniond.RotateTowards(ad, bd, p);

            NUnit.Framework.Assert.True(Approximate(value, valued));
        }
    }

    [Test]
    [Category("Quaterniond")]
    public void Slerp()
    {
        for (int i = 0; i < count; i++)
        {
            UnityEngine.Quaternion a = new UnityEngine.Quaternion();
            UnityEngine.Quaternion b = new UnityEngine.Quaternion();
            Quaterniond ad = new Quaterniond();
            Quaterniond bd = new Quaterniond();

            float p = UnityEngine.Random.Range(-2F, 2F);

            RandomQuaternion(ref a, ref ad);
            RandomQuaternion(ref b, ref bd);

            UnityEngine.Quaternion value = UnityEngine.Quaternion.Slerp(a, b, p);
            Quaterniond valued = Quaterniond.Slerp(ad, bd, p);

            NUnit.Framework.Assert.True(Approximate(value, valued));
        }
    }

    [Test]
    [Category("Quaterniond")]
    public void SlerpUnclamped()
    {
        for (int i = 0; i < count; i++)
        {
            UnityEngine.Quaternion a = new UnityEngine.Quaternion();
            UnityEngine.Quaternion b = new UnityEngine.Quaternion();
            Quaterniond ad = new Quaterniond();
            Quaterniond bd = new Quaterniond();

            float p = UnityEngine.Random.Range(2F, 3F);

            RandomQuaternion(ref a, ref ad);
            RandomQuaternion(ref b, ref bd);

            UnityEngine.Quaternion value = UnityEngine.Quaternion.SlerpUnclamped(a, b, p);
            Quaterniond valued = Quaterniond.SlerpUnclamped(ad, bd, p);

            NUnit.Framework.Assert.True(Approximate(value, valued));
        }
    }

    [Test]
    [Category("Quaterniond")]
    public void ToAngleAxis()
    {
        for (int i = 0; i < count; i++)
        {
            UnityEngine.Vector3 a;
            Vector3d b;
            float aa;
            double bb;

            UnityEngine.Quaternion quat = new UnityEngine.Quaternion();
            Quaterniond quatd = new Quaterniond();

            RandomQuaternion(ref quat, ref quatd);

            quat.ToAngleAxis(out aa, out a);
            quatd.ToAngleAxis(out bb, out b);

            NUnit.Framework.Assert.True(Approximate(aa, bb) && Approximate(a, b));
        }
    }
    [Test]
    [Category("Quaterniond")]
    public void EulerAngles()
    {
        int jingduI = 10;
        float jingduF = 10f;
        float max = 0f;
        FixedPointMath.Vector3 maxvec1 = new FixedPointMath.Vector3();
        FixedPointMath.Vector3 maxvec2 = new FixedPointMath.Vector3();
        //for (int x = 0; x <= 360 * 100; ++x)
        {
            //for (int y = 0; y <= 360 * 100; ++y)
            {
                int x1 = 0;
                int y1 = 0;
                for (int z1 = 0; z1 <= 360 * jingduI; ++z1)
                {
                    UnityEngine.Quaternion a1 = UnityEngine.Quaternion.Euler(new UnityEngine.Vector3(x1 / jingduF, y1 / jingduF, z1 / jingduF));
                    FixedPointMath.Quaternion n1 = FixedPointMath.Quaternion.Euler(new FixedPointMath.Vector3(x1 / jingduF, y1 / jingduF, z1 / jingduF));

                    int x2 = 0;
                    int y2 = 0;
                    for (int z2 = 0; z2 <= 360 * jingduI; ++z2)
                    {
                        UnityEngine.Quaternion a2 = UnityEngine.Quaternion.Euler(new UnityEngine.Vector3(x2 / jingduF, y2 / jingduF, z2 / jingduF));
                        FixedPointMath.Quaternion n2 = FixedPointMath.Quaternion.Euler(new FixedPointMath.Vector3(x2 / jingduF, y2 / jingduF, z2 / jingduF));

                        float f1 = UnityEngine.Quaternion.Angle(a1, a2);
                        float f2 = FixedPointMath.Quaternion.Angle(n1, n2).AsFloat();
                        float abs = Mathf.Abs(f1 - f2);
                        if (max < abs)
                        {
                            max = abs;
                            maxvec1.Set(x1 / jingduF, y1 / jingduF, z1 / jingduF);
                            maxvec2.Set(x2 / jingduF, y2 / jingduF, z2 / jingduF);
                        }
                    }
                }
            }
        }
        Debug.LogFormat("{0} {1} {2}", max, maxvec1, maxvec2);
        //float max = 0f;
        //NumberVec3 maxvec1 = new NumberVec3();
        //NumberVec3 maxvec2 = new NumberVec3();
        //for (int i = 0; i < count; i++)
        //{
        //    Quaternion a1 = new Quaternion();
        //    Quaternion a2 = new Quaternion();
        //    NumberVec3 vec1;
        //    NumberQuaternion aq1 = new NumberQuaternion();
        //    NumberQuaternion aq2 = new NumberQuaternion();
        //    NumberVec3 vec2;

        //    RandomQuaternion(ref a1, ref aq1, out vec1);
        //    RandomQuaternion(ref a2, ref aq2, out vec2);

        //    var f1 = Quaternion.Angle(a1, a2);
        //    var f2 = NumberQuaternion.Angle(aq1, aq2).AsFloat();

        //    //MyAssert(a1, aq1, Mathf.Abs(f1 - f2) < 0.001f);
        //    var abs = Mathf.Abs(f1 - f2);
        //    if (max < abs)
        //    {
        //        max = abs;
        //        maxvec1 = vec1;
        //        maxvec2 = vec2;
        //    }
        //    if (abs > 0.1f)
        //    {
        //        Debug.LogFormat("{0} {1} {2} {3} {4}", vec1, vec2, f1, f2, abs);
        //    }
        //}
        //Debug.LogFormat("{0} {1} {2}", maxvec1, maxvec2, max);
    }

    private void MyAssert(UnityEngine.Quaternion v, FixedPointMath.Quaternion aq, bool result)
    {
        if (!result)
        {
            NUnit.Framework.Assert.Fail(string.Format("{0}\n{1}", v.ToString("0.00000"), aq.ToString()));
            Debug.LogErrorFormat("{0} {1}", v, aq);
        }
        Assert.True(result);
    }

    private void RandomQuaternion(ref UnityEngine.Quaternion quat, ref Quaterniond quatd) {
        float ax, ay, az;

        ax = UnityEngine.Random.Range(0, 360F);
        ay = UnityEngine.Random.Range(0, 360F);
        az = UnityEngine.Random.Range(0, 360F);

        quat = UnityEngine.Quaternion.Euler(new UnityEngine.Vector3(ax, ay, az));
        quatd = Quaterniond.Euler(new Vector3d(ax, ay, az));
    }

    private void RandomQuaternion(ref UnityEngine.Quaternion quat, ref FixedPointMath.Quaternion q, out FixedPointMath.Vector3 vec3)
    {
        float ax, ay, az;

        ax = UnityEngine.Random.Range(0, 360F);
        ay = UnityEngine.Random.Range(0, 360F);
        az = UnityEngine.Random.Range(0, 360F);
        vec3 = new FixedPointMath.Vector3(ax, ay, az);

        quat = UnityEngine.Quaternion.Euler(new UnityEngine.Vector3(ax, ay, az));
        q = FixedPointMath.Quaternion.Euler(vec3);
    }

    private bool Approximate(UnityEngine.Vector3 v, Vector3d vd)
    {
        if (Math.Abs(v.x - vd.x) < deviation &&
            Math.Abs(v.y - vd.y) < deviation &&
            Math.Abs(v.z - vd.z) < deviation)
        {
            return true;
        }
        else
        {
            NUnit.Framework.Assert.Fail(string.Format("{0}\n{1}", v.ToString("0.00000"), vd.ToString("0.00000")));
            return false;
        }
    }

    private bool Approximate(UnityEngine.Vector3 v, FixedPointMath.Vector3 vq)
    {
        if (Math.Abs(v.x - vq.x.AsFloat()) < deviation &&
            Math.Abs(v.y - vq.y.AsFloat()) < deviation &&
            Math.Abs(v.z - vq.z.AsFloat()) < deviation)
        {
            return true;
        }
        else
        {
            NUnit.Framework.Assert.Fail(string.Format("{0}\n{1}", v.ToString("0.00000"), vq.ToString()));
            return false;
        }
    }

    private bool Approximate(UnityEngine.Quaternion v, Quaterniond vd)
    {
        if (Math.Abs(v.x - vd.x) < deviation &&
            Math.Abs(v.y - vd.y) < deviation &&
            Math.Abs(v.z - vd.z) < deviation &&
            Math.Abs(v.w - vd.w) < deviation)
        {
            return true;
        }
        else
        {
            NUnit.Framework.Assert.Fail(string.Format("{0}\n{1}", v.ToString("0.00000"), vd.ToString("0.00000")));
            return false;
        }
    }

    private bool Approximate(float v, double vd)
    {
        if (Math.Abs(v - vd) < deviation)
        {
            return true;
        }
        else
        {
            NUnit.Framework.Assert.Fail(string.Format("{0}\n{1}", v.ToString("0.00000"), vd.ToString("0.00000")));
            return false;
        }
    }
}
